/**
 * \file least_square.h
 * \author Zhan Zhihao
 * \brief 最小二乘法实现
 * \version 0.1
 * \date 2022-08-15
 *
 * @copyright Copyright (c) 2022
 *
 */

#ifndef LEAST_SQUARE_H
#define LEAST_SQUARE_H

#include <eigen3/Eigen/Dense>
#include <eigen3/Eigen/Sparse>

#include <iostream>
#include <iomanip>
#include <cmath>

const double DERIV_STEP = 1e-5;
const int    MAX_ITER   = 100;

#define max(a, b) (((a) > (b)) ? (a) : (b))

class LeastSquare {
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  public:
  /**
   * \brief Construct a new Least Square object
   *
   */
  LeastSquare();

  /**
   * \brief Destroy the Least Square object
   *
   */
  ~LeastSquare();

  /**
   * \brief 单个残差
   *
   * \param input 输入
   * \param output 输出
   * \param params 参数
   * \param index 索引
   * \return 返回残差值
   */
  double CostFunc(const Eigen::VectorXd &input,
                  const Eigen::VectorXd &output,
                  const Eigen::VectorXd &params,
                  int                    index);

  /**
   * \brief return vector make up of CostFunc() element
   *
   * \param input 输入
   * \param output 输出
   * \param params 参数
   */
  void ObjF(const Eigen::VectorXd &input, const Eigen::VectorXd &output, const Eigen::VectorXd &params);

  /**
   * \brief 计算目标矩阵的二范数
   *
   * \param obj
   * \return 返回矩阵的二范数
   */
  double Func(const Eigen::VectorXd &obj)
  {
    return obj.squaredNorm() / 2;
  }

  /**
   * \brief 求导
   *
   * \param input 输入
   * \param output 输出
   * \param index 索引
   * \param params 参数
   * \param p_index 参数索引
   * \return 返回偏导
   */
  double Deriv(const Eigen::VectorXd &input,
               const Eigen::VectorXd &output,
               int                    index,
               const Eigen::VectorXd &params,
               int                    p_index);

  /**
   * \brief 雅克比计算
   *
   * \param input 输入
   * \param output 输出
   * \param params 参数
   * \return 返回雅克比矩阵
   */
  void Jacobian(const Eigen::VectorXd &input, const Eigen::VectorXd &output, const Eigen::VectorXd &params);

  /**
   * \brief Gauss-Newton
   *
   * \param input 输入
   * \param output 输出
   * \param params 参数
   */
  void GaussNewton(const Eigen::VectorXd &input, const Eigen::VectorXd &output, Eigen::VectorXd &params);

  /**
   * \brief 求矩阵对角线的最大值
   *
   * \param Hessian 海森矩阵
   * \return 对角线最大值
   */
  double MaxMatrixDiag(const Eigen::MatrixXd &Hessian);

  /**
   * \brief L(h) = F(x) + h^t*J^t*f + h^t*J^t*J*h/2
   *        deltaL = L(0)-L(h_lm)
   *
   * \param h
   * \return L(0)-L(h)
   */
  double DeltaL(const Eigen::VectorXd &h);

  /**
   * \brief LM求解
   *
   * \param input 输入
   * \param output 输出
   * \param params 参数
   */
  void LM(const Eigen::VectorXd &input, const Eigen::VectorXd &output, Eigen::VectorXd &params);

  /**
   * \brief Dog-Leg求解
   *
   * \param input 输入
   * \param output 输出
   * \param params 参数
   */
  void DogLeg(const Eigen::VectorXd &input, const Eigen::VectorXd &output, Eigen::VectorXd &params);

  private:
  Eigen::MatrixXd fx_; // residual
  Eigen::MatrixXd J_;  // jacbian
};

#endif // LEAST_SQUARE_H